#include "../include/torsodetector_vis/visualizer.h"

TorsoVisualizer::TorsoVisualizer():nh()
{

}

bool TorsoVisualizer::init()
{
    try
    {
        torsoSubscriber= nh.subscribe<hri_msgs::Torsos>("/torsos",1,
                                                        (boost::function<void(const hri_msgs::TorsosConstPtr&)>)
                                                        boost::bind(&TorsoVisualizer::torsoSubscriberCB, this, _1));

        torso_marker_publisher=nh.advertise<visualization_msgs::MarkerArray>("/torsos_marker",100);

        transform_listener= new tf::TransformListener(ros::Duration(1000));
    }
    catch(...)
    {
        return false;
    }
    return true;

}

int TorsoVisualizer::run()
{
    //    ros::Rate loop_rate(100);
    //    while(ros::ok())
    //    {
    //        ros::spinOnce();
    //        loop_rate.sleep();
    //    }
    ros::spin();
    return 0;

}

void TorsoVisualizer::torsoSubscriberCB(const hri_msgs::TorsosConstPtr& torsos)
{
    visualization_msgs::MarkerArray marker_array;
    int id = 1000;
    for (uint i = 0; i < torsos->torsos.size(); i++)
    {
        /*if(torsos->torsos[i].confidence.data >= 0.5)
        {*/
            marker_array.markers.push_back(createMarker(torsos->torsos[i].center_of_torso,id));
            id++;
        //}
    }
    torso_marker_publisher.publish(marker_array);
}

geometry_msgs::PoseStamped TorsoVisualizer::transform(geometry_msgs::Pose pose)
{
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.pose=pose;
    pose_stamped.pose.orientation.w = 1;
    pose_stamped.pose.orientation.x = 0;
    pose_stamped.pose.orientation.y = 0;
    pose_stamped.pose.orientation.z = 0;

    pose_stamped.header.frame_id="/ubg_laser";
    pose_stamped.header.stamp=ros::Time::now();
    geometry_msgs::PoseStamped transformed_pose;
    /*try
    {
        if(transform_listener->waitForTransform("/base_link","/laser",ros::Time(),ros::Duration(50)))
        {
            transform_listener->transformPose("/base_link",pose_stamped,transformed_pose);
            std::cout<<transformed_pose;
        }
    }
    catch(tf::ExtrapolationException e)
    {
    }*/
    transformed_pose = pose_stamped;
    return transformed_pose;
}

visualization_msgs::Marker TorsoVisualizer::createMarker(geometry_msgs::Pose torso_pose, int id)
{
    visualization_msgs::Marker marker;
    marker.pose=transform(torso_pose).pose;
    marker.header.frame_id = "/ubg_laser";
    marker.header.stamp = ros::Time::now();
    marker.ns = "detected_torsos";
    marker.id = id;
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1;
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 1.0f;
    marker.color.a = 1.0f;

    marker.scale.x = 0.05;
    marker.scale.y = 0.05;
    marker.scale.z = 0.05;
    marker.text = "Human_torso";
    marker.lifetime = ros::Duration();
    return marker;
}
